/* Copyright (c) 2007 Scott Lembcke
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
 
#include <stdlib.h>
#include <float.h>
#include <stdarg.h>

#include "chipmunk_private.h"

// initialized in cpInitChipmunk()
cpBody cpStaticBodySingleton;

cpBody*
cpBodyAlloc(void)
{
	return (cpBody *)cpmalloc(sizeof(cpBody));
}

cpBodyVelocityFunc cpBodyUpdateVelocityDefault = cpBodyUpdateVelocity;
cpBodyPositionFunc cpBodyUpdatePositionDefault = cpBodyUpdatePosition;

cpBody *
cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
{
	body->velocity_func = cpBodyUpdateVelocityDefault;
	body->position_func = cpBodyUpdatePositionDefault;
	
	cpBodySetMass(body, m);
	cpBodySetMoment(body, i);

	body->p = cpvzero;
	body->v = cpvzero;
	body->f = cpvzero;
	
	cpBodySetAngle(body, 0.0f);
	body->w = 0.0f;
	body->t = 0.0f;
	
	body->v_bias = cpvzero;
	body->w_bias = 0.0f;
	
	body->data = NULL;
	body->v_limit = (cpFloat)INFINITY;
	body->w_limit = (cpFloat)INFINITY;
	
	body->space = NULL;
	body->shapesList = NULL;
	
	cpComponentNode node = {NULL, NULL, 0, 0.0f};
	body->node = node;
	
	return body;
}

cpBody*
cpBodyNew(cpFloat m, cpFloat i)
{
	return cpBodyInit(cpBodyAlloc(), m, i);
}

cpBody *
cpBodyInitStatic(cpBody *body)
{
	cpBodyInit(body, (cpFloat)INFINITY, (cpFloat)INFINITY);
	body->node.idleTime = (cpFloat)INFINITY;
	
	return body;
}

cpBody *
cpBodyNewStatic()
{
	return cpBodyInitStatic(cpBodyAlloc());
}

void cpBodyDestroy(cpBody *body){}

void
cpBodyFree(cpBody *body)
{
	if(body){
		cpBodyDestroy(body);
		cpfree(body);
	}
}

void
cpBodySetMass(cpBody *body, cpFloat mass)
{
	body->m = mass;
	body->m_inv = gD(1.0f, mass);
}

void
cpBodySetMoment(cpBody *body, cpFloat moment)
{
	body->i = moment;
	body->i_inv = gD(1.0f, moment);
}

void
cpBodySetAngle(cpBody *body, cpFloat angle)
{
	body->a = angle;//fmod(a, (cpFloat)M_PI*2.0f);
	body->rot = cpvforangle(angle);
}

void
cpBodySlew(cpBody *body, cpVect pos, cpFloat dt)
{
	cpVect delta = cpvsub(pos, body->p);
	body->v = cpvmult(delta, gD(1.0f, dt));
}

void
cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
{
	body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)), body->v_limit);
	
	cpFloat w_limit = body->w_limit;
	body->w = cpfclamp(gA(gM(body->w, damping), gM3(body->t, body->i_inv, dt)), -w_limit, w_limit);
}

void
cpBodyUpdatePosition(cpBody *body, cpFloat dt)
{
	body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt));
	cpBodySetAngle(body, gA(body->a, gM(gA(body->w, body->w_bias), dt)));
	
	body->v_bias = cpvzero;
	body->w_bias = 0.0f;
}

void
cpBodyUpdateMassMoment(cpBody *body)
{
	cpFloat mass = 0.0f;
	cpVect center = cpvzero;
	cpFloat mom = 0.0f;

	// Calculate center of mass and total mass

	for(cpShape *shape = body->shapesList; shape; shape = shape->next) {
		switch(shape->klass->type) {
			case CP_CIRCLE_SHAPE: {
				cpCircleShape *circle = (cpCircleShape *)shape;
				cpFloat circleMass = gM(cpAreaForCircle(0.0f, circle->r), shape->density);
				mass = gA(mass, circleMass);
				center = cpvadd(center, cpvmult(circle->c, circleMass));
				mom = gA(mom, cpMomentForCircle(circleMass, 0.0f, circle->r, circle->c));
				break;
			}

			case CP_SEGMENT_SHAPE: {
				cpSegmentShape *segment = (cpSegmentShape *)shape;
				cpFloat segmentMass = gM(cpAreaForSegment(
					segment->CP_PRIVATE(a), segment->CP_PRIVATE(b), segment->CP_PRIVATE(r)),
					shape->density);
				mass = gA(mass, segmentMass);
				center = cpvadd(center, cpvmult(
					cpvmult(cpvadd(segment->CP_PRIVATE(a), segment->CP_PRIVATE(b)), 0.5f),
					segmentMass));
				mom = gA(mom, cpMomentForSegment(segmentMass, segment->CP_PRIVATE(a), segment->CP_PRIVATE(b)));
				break;
			}
		}
	}

	if (mass > 0.0f) {
		cpBodySetMass(body, mass);
		center = cpvmult(center, body->m_inv);
	} else {
		cpBodySetMass(body, 1.0f);
	}

	if (mom > 0.0f) {
		mom = gS(mom, gM(body->m, cpvdot(center, center)));
		cpBodySetMoment(body, mom);
	} else {
		cpBodySetMoment(body, 1.0f);
	}

	// TODO from Box2D: m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);

	// Reposition

	body->p = cpvadd(body->p, cpvrotate(center, body->rot));
	
	for(cpShape *shape = body->shapesList; shape; shape = shape->next) {
		switch(shape->klass->type) {
			case CP_CIRCLE_SHAPE: {
				cpCircleShape *circle = (cpCircleShape *)shape;
				circle->c = cpvsub(circle->c, center);
				break;
			}

			case CP_SEGMENT_SHAPE: {
				cpSegmentShape *segment = (cpSegmentShape *)shape;
				segment->a = cpvsub(segment->a, center);
				segment->b = cpvsub(segment->b, center);
				break;
			}
		}

		/*
		CP_CIRCLE_SHAPE,
		CP_SEGMENT_SHAPE,
		CP_POLY_SHAPE,

		cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset);
		cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2);
		cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b);
		cpFloat cpAreaForSegment(cpVect a, cpVect b, cpFloat r);
		cpFloat cpMomentForPoly(cpFloat m, int numVerts, const cpVect *verts, cpVect offset);
		cpFloat cpAreaForPoly(const int numVerts, const cpVect *verts);
		cpVect cpCentroidForPoly(const int numVerts, const cpVect *verts);
		void cpRecenterPoly(const int numVerts, cpVect *verts);
		cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height);*/
	}

	
}

void
cpBodyResetForces(cpBody *body)
{
	body->f = cpvzero;
	body->t = 0.0f;
}

void
cpBodyApplyForce(cpBody *body, cpVect force, cpVect r)
{
	body->f = cpvadd(body->f, force);
	body->t = gA(body->t, cpvcross(r, force));
}

void
cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt)
{
	// Calculate the world space anchor coordinates.
	cpVect r1 = cpvrotate(anchr1, a->rot);
	cpVect r2 = cpvrotate(anchr2, b->rot);
	
	cpVect delta = cpvsub(cpvadd(b->p, r2), cpvadd(a->p, r1));
	cpFloat dist = cpvlength(delta);
	cpVect n = dist ? cpvmult(delta, gD(1.0f, dist)) : cpvzero;
	
	cpFloat f_spring = gM(gS(dist, rlen), k);

	// Calculate the world relative velocities of the anchor points.
	cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
	cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
	
	// Calculate the damping force.
	// This really should be in the impulse solver and can produce problems when using large damping values.
	cpFloat vrn = cpvdot(cpvsub(v2, v1), n);
	cpFloat f_damp = gM(vrn, cpfmin(dmp, gD(1.0f, gM(dt,gA(a->m_inv, b->m_inv)))));
	
	// Apply!
	cpVect f = cpvmult(n, gA(f_spring, f_damp));
	cpBodyApplyForce(a, f, r1);
	cpBodyApplyForce(b, cpvneg(f), r2);
}
